//
// Created by liangwenhao on 2024/4/4.
//

#ifndef _PID_CALCULATION_H
#define _PID_CALCULATION_H
#include "stdio.h"
#include "stdbool.h"
typedef struct Motor_t
{
    struct
    {
        float Kp;
        float Ki;
        float Kd;
        float Out;
        float POut;
        float IOut;
        float IOut_Max;
        float DOut;
        float Target;
        float Real;
        float err;
        float err_last;
        float err_sum;
    }PID_Speed;

    struct
    {
        float Kp;
        float Ki;
        float Kd;
        float Out;
        float POut;
        float IOut;
        float IOut_Max;
        float DOut;
        float Target;
        float Real;
        float err;
        float err_last;
        float err_sum;
        float Angle_begin;//记录初始角度
        float round_num;//记录圈数
        float round_num_last;
        float Cross;//转过的角度
    }PID_Angle;

    struct
    {
        int16_t StdID;///can接收电机数据的ID
        uint8_t Number;///can发送电机数据的编号
        uint8_t Type;///电机类型
        float real_speed;
        float real_speed_last;
        float target_speed;
        float real_angle;
        float real_angle_last;

        float target_angle;
        float torque_current;
        float torque_current_max;
        float motor_shaking_cross;
        int16_t current;//输出电流

    }Motor_Data;

    struct
    {
        int Angle_begin_record;//电机记录位置环初始位置标志位
        int Stalled;//电机堵转标志位
        int Crazy;//电机疯转标志位
        int Reset;//电机复位标志位
        int Shaking;//电机抖动标志位
        int over_num;
    }Flag;

    struct
    {
        float count;
        float count_max;
    }Slope;
}Motor;

typedef struct
{
    float Kp;
    float Ki;
    float Kd;
    float Out;
    float POut;
    float IOut;
    float IOut_Max;
    float DOut;
    float Target;
    float Real;
    float err;
    float err_last;
    float err_sum;
}PID;
void PIDCalculation_Speed(Motor *motor);
void PIDCalculation_Angle(Motor *motor);
void PIDCalculation(PID *pid);
void PID_Init(PID *pid,float p,float i,float d);
#endif //_PID_CALCULATION_H
